% TrajectoryGeneration.m

function trajectory = TrajectoryGeneration()
% res 2 * n 第一行x 第二行y
    load("ConstantDefinition.mat","LINE_LEN","R", "MOTION1_OFFSET", "MOTION2_OFFSET", "rM","FrameRate","speed");
    step = speed / FrameRate;       % 每次增加路程
    initR = [rM(1) + LINE_LEN, rM(2) + R];     % 初始点位置
    len = ceil((LINE_LEN + 2*pi*R) / step);
    trajectory = zeros([2 len]);
    x = initR(1);
    y = initR(2);
    t = 1;

    % 直线部分
    cir = [rM(1), rM(2)];     % 第一个圆心
    while x > rM(1)
        trajectory(1,t) = x;
        trajectory(2,t) = y;
        t = t + 1;
        x = x - step;
    end

    theta = (cir(1) - x) / R;
    while theta < 2 * pi
        trajectory(1,t) = cir(1) - R*sin(theta);
        trajectory(2,t) = cir(2) + R*cos(theta);
        theta = theta + step / R;
        t = t + 1;
    end

    scatter(trajectory(1,:), trajectory(2,:));
    ylim([40 140])
    xlim([40 140])
    save("TrajectoryData.mat", "trajectory");

    theta_init = initalposition();
    angel = zeros([2 len]);
    for i = 1 : len
        theta = InvPositionSov(trajectory(:, i), theta_init);
        theta_init = theta;
        angel(1, i) = theta(1);
        angel(2, i) = theta(4);
    end

    % 处理角度数据，由默认极坐标系变换至连杆电机坐标系
    angel(1,:) = angel(1,:) + MOTION1_OFFSET;
    angel(2,:) = angel(2,:) + MOTION2_OFFSET;
    angel = mod(angel, 2*pi);
    Angel_theta24 = floor(angel * 2048 / pi);
    save("ThetaData.mat", "Angel_theta24");
end